#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import time
import rospy

from arm import Arm
from nav import Nav
from audio import Audio
from robot_twist import RobotTwist


class Robot():
    def __init__(self):
        self.arm = Arm()
        self.robot_twist = RobotTwist()
        self.nav = Nav()
        # self.audio = Audio()

    def start(self):
        # self.audio.say('道路千万条，安全第一条，行车不规范，亲人两行泪')
        # self.nav.nav_pick()
        # time.sleep(2)
        # self.audio.say('你是什么垃圾')
        # self.arm.arm_detect()
        # time.sleep(3)
        # self.audio.say('可回收物')
        rospy.loginfo('[robot] forward...')
        self.robot_twist.forward(0.65)
        time.sleep(3)
        rospy.loginfo('[robot] arm pick...')
        self.arm.arm_pick()
        rospy.loginfo('[robot] nav place...')
        self.nav.nav_place()
        # rospy.loginfo('[robot] rotate...')
        # self.robot_twist.rotate(3)
        # time.sleep(3)
        rospy.loginfo('[robot] arm place...')
        self.arm.arm_place()
        rospy.loginfo('[robot] nav stanby...')
        self.nav.nav_stanby()
        rospy.loginfo('[robot] arm detect...')
        self.arm.arm_detect()
        rospy.loginfo('[robot] done!')
        # self.audio.say('拜了个拜')


if __name__ == '__main__':
    rospy.init_node('npu_robot', anonymous=True, log_level=envs.log_level)
    robot = Robot()
    robot.start()
